/*	
 *				
 *					ISNW ---- It is a war.It is not warcraft!
 *
 *
 *					Author:					zhoukeli(haxixi_keli)
 *					Filename:				quaternion.h
 *					CreateTime:				2008/12/20
 *					ModifyTime:				2008/12/20
 *					ModifyTime:				2008/12/23
 */

#ifndef __ISNW_QUATERNION_H__
#define __ISNW_QUATERNION_H__

#include "isnwTypes.h"
#include "matrix4.h"
#include "isnwMath.h"

namespace ISNW
{
namespace core
{
	template<typename T>
	class quaternion
	{
	public:
		//! default constructor
		quaternion():x(0),y(0),z(0),w(0){}
		//! copy constructor
		quaternion(const quaternion<T>& other)
			:x(other.x),y(other.y),z(other.z),w(other.w){}

		//! constructor
		quaternion(T tx,T ty,T tz,T tw):x(tx),y(ty),z(tz),w(tw){}

		quaternion(const matrix4<T>& other)	{set(other);}

	public:
		//! operator
		bool operator == (const quaternion<T>& other) const
		{
			return (x == other.x) && (y == other.y) && (z == other.z) && (w == other.w);
		}

		bool operator != (const quaternion<T>& other) const
		{
			return !(*this == other);
		}

		quaternion<T>& operator = (const quaternion<T>& other)
		{
			set(other);

			return *this;
		}

		quaternion<T>& operator = (const matrix4<T>& other)
		{
			set(other);

			return *this;
		}

		quaternion<T>& operator += (const quaternion<T>& other)
		{
			x += other.x;
			y += other.y;
			z += other.z;
			w += other.w;

			return *this;
		}

		quaternion<T> operator + (const quaternion<T>& other) const
		{
			return quaternion<T>(x+other.x,y+other.y,z+other.z,w+other.w);
		}

		quaternion<T>& operator *= (T v)
		{
			x *= v;
			y *= v;
			z *= v;
			w *= v;

			return *this;
		}

		quaternion<T> operator * (T v) const
		{
			return quaternion<T>(x*v,y*v,z*v,w*v);
		}

		quaternion<T> operator - () const
		{
			return quaternion<T>(-x,-y,-z,-w);
		}

		quaternion<T>& operator -= (const quaternion<T>& other)
		{
			*this += -other;

			return this;
		}

		quaternion<T> operator - (const quaternion<T>& other) const
		{
			return quaternion<T>(x-other.x,y-other.y,z-other.z,w-other.w);
		}

		quaternion<T>& operator *= (const quaternion<T>& other)
		{
			quaternion<T> tmp = *this;

			w = tmp.w*other.w - tmp.x*other.x - tmp.y*other.y - tmp.z*other.z;
			x = tmp.w*other.x + tmp.x*other.w + tmp.y*other.z - tmp.z*other.y;
			y = tmp.w*other.y - tmp.x*other.z + tmp.y*other.w + tmp.z*other.x;
			z = tmp.w*other.z + tmp.x*other.y - tmp.y*other.x + tmp.z*other.w;

			return *this;
		}

		quaternion<T> operator * (const quaternion<T>& other) const
		{
			quaternion<T> tmp = *this;
			tmp *= other;

			return tmp;
		}

		f32 getlenghtSQ() const
		{
			f32 len = x*x + y*y + z*z + w*w;
			return len;
		}

		f32 getlength() const
		{
			f32 len = getlenghtSQ();

			if(len == 0) return 0;

			return core::_sqrt_(len);
		}

		core::matrix4<T> getmatrix() const
		{
			matrix4<T> mat;

			mat[0] = 1 - 2 * (y*y + z*z);
			mat[1] = 2 * (x*y - w*z);
			mat[2] = 2 * (w*y + x*z);
			mat[3] = 0;

			mat[4] = 2 * (x*y + w*z);
			mat[5] = 1 - 2 * (x*x + z*z);
			mat[6] = 2 * (y*z - w*x);
			mat[7] = 0;

			mat[8] = 2 * (x*z - w*y);
			mat[9] = 2 * (y*z + w*x);
			mat[10] = 1 - 2 * (x*x + y*y);
			mat[11] = 0;

			mat[12] = 0;
			mat[13] = 0;
			mat[14] = 0;
			mat[15] = 1;

			return mat;
		}

		void getmatrix(core::matrix4<T>& mat) const
		{
			mat = getmatrix();

			return;
		}

		quaternion<T>& setfromaxisandangle(core::vector3d<T> axis,f32 angle)
		{
			f32 halfangle = 0.5f * angle;

			w = core::_cos_(halfangle);

			f32 len = axis.getlength();
			if(len == 0)
			{
				w = 1;
				x = y = z = 0;

				return;
			}

			axis /= len;

			f32 sn = core::_sin_(halfangle);

			x = sn * axis.x;
			y = sn * axis.y;
			z = sn * axis.z;

			return *this;
		}

		void toaxisandangle(core::vector3d<T>& axis,f32& angle)
		{
			f32 scale = getlength();

			if(core::_equal_(scale,0))
			{
				axis = core::vector3d<T>(0,1,0);
				angle = 0;
				return;
			}

			axis.x = x / scale;
			axis.y = y / scale;
			axis.z = z /scale;

			angle = 2.0f * core::_arccos_(w);

			return;
		}

		void normalize()
		{
			f32 len = getlength();

			if(core::_equal_(len,1.0f)) return;

			if(core::_equal_(len,0.0f)) return;

			x /= len;
			y /= len;
			z /= len;

			w /= len;

			return;
		}

		quaternion<T>& makeinverse()
		{
			x = -x;
			y = -y;
			z = -z;

			return *this;
		}

		quaternion<T> inverse() const
		{
			return quaternion<T>(-x,-y,-z,w);
		}

	public:
		//! function
		quaternion<T>& set(T tx,T ty,T tz,T tw)
		{
			x = tx;
			y = ty;
			z = tz;
			w = tw;

			normalize();

			return *this;
		}

		quaternion<T>& set(const quaternion<T>& other)
		{
			set(other.x,other.y,other.z,other.w);

			return *this;
		}

		quaternion<T>& set(const matrix4<T>& other)
		{
			/* The special matrix is like this:
				
				1 - 2(y*y + z*z)		2(x*y - w*z)		2(w*y + x*z)		0

				2(x*y + w*z)			1 - 2(x*x + z*z)	2(y*z - w*x)		0

				2(x*z - w*y)			2(y*z + w*x)		1 - 2(x*x + y*y)	0

				0						0					0					1

				so:
					m(0,0) + m(1,1) + m(2,2) + m(3,3) = 4 - 4(x*x + y*y + z*z)

				as we know:
					x*x + y*y + z*z + w*w = 1;

				so that:
					m(0,0) + m(1,1) + m(2,2) + m(3,3) = 4 - 4(1 - w*w) = 4*w*w

					w = sqrt(m(0,0) + m(1,1) + m(2,2) + m(3,3)) * 0.5
			*/

			f32 diag = other(0,0) + other(1,1) + other(2,2) + other(3,3);

			if(diag <= 0)
			{
				x = y = z = w =0;
				return *this;
			}

			w = core::_sqrt_(diag) * 0.5;

			x = (other(2,1) - other(1,2)) * 0.25 / w;
			y = (other(0,2) - other(2,0)) * 0.25 / w;
			z = (other(1,0) - other(0,1)) * 0.25 / w;



			return *this;
		}

	public:
		T x;
		T y;
		T z;
		T w;
	};

	//! typedef for float quaternion
	typedef quaternion<f32> quaternionf;
	//! typedef for int quaternion
	typedef quaternion<s32> quaternioni;

} // end namespace core
} // end namespace ISNW
#endif